#include "tiov3.h"

#include <assert.h>
#include <string.h>

#include "log/zlog.hh"

#include "rtdev/rtdev.h"
#include "rtdev/tio/tio_intf.h"
#include "rtbus/rtbus.h"
#include "jkutil/configparser.h"

int TioV3SysIoItf::set_tio_RGB(TioLampColor color)
{  //part of PDO 0X210
    cmd_RGB_ = color;
    return 0;
}

int TioV3SysIoItf::get_tio_RGB() { return fb_RGB_; }

TioKeys TioV3SysIoItf::get_tio_keys() { return fb_keys_; }

int TioV3SysIoItf::query_Tio_version()
{
    int err = tiov3->get_sdo_blocking(TIO_VERSION_R, &data_, 4, 40);
    if (err)
    {
        return err;
    }
    hw_version_ = data_ >> 16 & 0xffff;
    sw_version_ = data_ & 0xffff;
    return 0;
    //todo:非周期性指令的实现 OBJID_VERSION
}

uint32_t TioV3SysIoItf::get_tio_version() { return data_; }

//paramcode 0x0301
// 0 for power off, 1 for power on
int TioV3SysIoItf::tio_set_vout_enable(uint32_t cfg)
{
    cmd_cfg_.cfg_vout_enable = cfg;
    int err = tiov3->set_sdo_blocking(TIO_VOUT_ENABLE_RW, cmd_cfg_.cfg_vout_enable, 4, &fb_cfg_.cfg_vout_enable);
    if (err)
    {
        return err;
    }
    return 0;
}
int TioV3SysIoItf::tio_get_vout_enable() { return fb_cfg_.cfg_vout_enable; }

//paramcode 0x0100
//  0 for 24V, 1 for 12V
int TioV3SysIoItf::tio_set_vout_voltage(uint32_t data)
{
    cmd_cfg_.cfg_vout_voltage = data;
    int err = tiov3->set_sdo_blocking(TIO_OUT_VOLTAGE_RW, cmd_cfg_.cfg_vout_voltage, 4, &fb_cfg_.cfg_vout_voltage);
    if (err)
    {
        return err;
    }
    return 0;
}
int TioV3SysIoItf::tio_get_vout_voltage() { return fb_cfg_.cfg_vout_voltage; }

int TioV3UsrIoItf::get_di(std::vector<int8_t>& output)
{
    std::vector<int8_t> outArray;
    for (int i = 0; i < di_cnt__; i++) { outArray.push_back((stat_.di_buffer >> i) & 0b1); }
    output.assign(outArray.begin(), outArray.end());
    return 0;
}
int TioV3UsrIoItf::set_do(std::vector<int8_t> input)
{
    if (stat_.do_config == 0xff)
    {
        return -1;
    }
    uint8_t do_buffer = 0;  //整个组装完成后 再赋值，避免非原子操作导致的do跳变
    for (int i = 0; i < do_cnt__; i++) { do_buffer += input[i] << i; }
    cmd_.set_do = do_buffer;
    return 0;
}
int TioV3UsrIoItf::get_do(std::vector<int8_t>& output)
{
    std::vector<int8_t> outArray;
    for (int i = 0; i < do_cnt__; i++) { outArray.push_back((stat_.do_buffer >> i) & 0x1); }
    output.assign(outArray.begin(), outArray.end());
    return 0;
}
int TioV3UsrIoItf::get_ai(std::vector<double>& output)
{
    std::vector<double> outArray;
    for (int i = 0; i < ai_cnt__; i++) { outArray.push_back(stat_.ai[i]); }
    output.assign(outArray.begin(), outArray.end());
    return 0;
}

bool TioV3UsrIoItf::is_tio_v3() { return true; }

int TioV3UsrIoItf::tio_setup_di_pin_mode(uint32_t cfg)
{
    cmd_.di_config = cfg;
    int err = tiov3->set_sdo_blocking(TIO_DI_CFG_RW, cmd_.di_config, 4, &stat_.di_config);
    if (err)
    {
        return err;
    }
    return 0;
}
int TioV3UsrIoItf::tio_setup_do_pin_mode(uint32_t cfg)
{
    cmd_.do_config = cfg;
    int err = tiov3->set_sdo_blocking(TIO_DO_CFG_RW, cmd_.do_config, 4, &stat_.do_config);
    if (err)
    {
        return err;
    }
    return 0;
}
int TioV3UsrIoItf::tio_setup_ai_pin_mode(uint32_t cfg)
{
    cmd_.ai_config = cfg;
    int err = tiov3->set_sdo_blocking(TIO_AI_CFG_RW, cmd_.ai_config, 4, &stat_.ai_config);
    if (err)
    {
        return err;
    }
    return 0;
}
int TioV3UsrIoItf::tio_get_di_pin_mode() { return stat_.di_config; }
int TioV3UsrIoItf::tio_get_do_pin_mode() { return stat_.do_config; }
int TioV3UsrIoItf::tio_get_ai_pin_mode() { return stat_.ai_config; }
int TioV3UsrIoItf::set_tio_rs485_chn_mode(int chn_id, RS485Mode mode)
{
    //根据chn配置0x104 0x105参数
    switch (chn_id)
    {
    case 0: {
        if (stat_.do_config != 0xff)  //仅do被配置为485后才可发送配置
        {
            return -1;
        }
        int err = tiov3->set_sdo_blocking(TIO_RS485_1_MOD_RW, mode, 4, &stat_rs485cfg_.rs485_mode_cfg[0]);
        if (err)
        {
            return err;
        }
        break;
    }
    case 1: {
        if (stat_.ai_config != 1)  //仅ai被配置为485后才可发送配置
        {
            return -1;
        }
        int err = tiov3->set_sdo_blocking(TIO_RS485_2_MOD_RW, mode, 4, &stat_rs485cfg_.rs485_mode_cfg[1]);
        if (err)
        {
            return err;
        }
        break;
    }

    default:
        return -1;
        break;
    }
    return 0;
}
int TioV3UsrIoItf::get_tio_rs485_chn_mode(int chn_id) { return stat_rs485cfg_.rs485_mode_cfg[chn_id]; }
int TioV3UsrIoItf::set_tio_rs485_chn_comm(int chn_id, RS485RTUConfig rtu_comm_params)
{  //根据chn配置0x106 0x107参数
    uint32_t comm_cfg = (rtu_comm_params.baudrate & 0x7FFFFF);
    uint32_t fb_comm_cfg = 0;
    if (rtu_comm_params.stopbits == 2)
    {
        comm_cfg |= (2 << 23);
    }
    if (rtu_comm_params.parity == 'E')
    {
        comm_cfg |= ((1 & 0x3) << 25);
    }
    else if (rtu_comm_params.parity == 'O')
    {
        comm_cfg |= ((2 & 0x3) << 25);
    }
    if (rtu_comm_params.databits == 9)  //todo RTU数据位应该是7或8   但硬件是8或9 目前按照通讯协议实现
    {
        comm_cfg |= (1 << 27);
    }
    switch (chn_id)
    {
    case 0: {
        int err = tiov3->set_sdo_blocking(TIO_RS485_1_COMM_RW, comm_cfg, 4, &fb_comm_cfg);
        if (err)
        {
            return err;
        }
        break;
    }

    case 1: {
        int err = tiov3->set_sdo_blocking(TIO_RS485_2_COMM_RW, comm_cfg, 4, &fb_comm_cfg);
        if (err)
        {
            return err;
        }
        break;
    }

    default:
        return -1;
        break;
    }
    stat_rs485cfg_.rs485_comm_cfg[chn_id].baudrate = comm_cfg & 0x7FFFFF;
    if ((comm_cfg & (2 << 23)) != 0)
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].stopbits = 2;
    }
    else
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].stopbits = 1;
    }
    uint8_t parity_bits = (comm_cfg >> 25) & 0x3;
    if (parity_bits == 1)
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].parity = 'E';
    }
    else if (parity_bits == 2)
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].parity = 'O';
    }
    else
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].parity = 'N';
    }
    if ((comm_cfg & (1 << 27)) != 0)
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].databits = 9;  // todo RTU数据位应该是7或8 但硬件是8或9 目前按照通讯协议实现
    }
    else
    {
        stat_rs485cfg_.rs485_comm_cfg[chn_id].databits = 8;
    }
    stat_rs485cfg_.rs485_comm_cfg[chn_id].slaveId = rtu_comm_params.slaveId;  // slave_id不会存储在tio侧,由控制器保存,用于信号量等485通讯组包
    return 0;
}
int TioV3UsrIoItf::get_tio_rs485_chn_comm(int chn_id, RS485RTUConfig& rtuComm)
{
    rtuComm = stat_rs485cfg_.rs485_comm_cfg[chn_id];
    return 0;
}
int TioV3UsrIoItf::tio_set_rs485_chn_func(uint32_t cfg)  //0x10a 力矩传感器通道选择  考虑改名
{
    if (((cfg == 1) && (stat_.do_config != 0xff)) || ((cfg == 2) && (stat_.ai_config != 0x1)))
    {
        return -1;  //仅ai\do被配置为485后才可发送配置
    }
    int err = tiov3->set_sdo_blocking(TIO_SENSOR_CHN_CFG_RW, cfg, 4, &stat_rs485cfg_.rs485_sensor_chn);
    if (err)
    {
        return err;
    }
    return 0;
}

/**
 * @brief 收集数据，检查分包完整性，更新了成员变量rs485_buffer_
 * 
 * @return True for success, False for failure
 */
bool TioV3UsrIoItf::collect_frame(uint8_t chn, uint8_t* data, uint8_t len)
{
    uint8_t whole_frame_size = data[0];
    uint8_t cur_frame_id = data[1];  // 从1计数

    if (whole_frame_size > MAX_RS485_FRAME_SIZE)
    {
        memset(&rs485_buffer_[chn], 0, sizeof(RS485Msg));
        return false;
    }

    if (cur_frame_id > whole_frame_size)
    {
        memset(&rs485_buffer_[chn], 0, sizeof(RS485Msg));
        return false;
    }

    if (whole_frame_size > 1)
    {
        //重复标号 假定为错误帧
        if (rs485_buffer_[chn].is_frame_part_in[cur_frame_id])
        {
            memset(&rs485_buffer_[chn], 0, sizeof(RS485Msg));
            return false;
        }
        else  // 如果 大标号先到， 假定为错误帧；
        {
            for (int i = 1; i < cur_frame_id; i++)
            {
                if (!rs485_buffer_[chn].is_frame_part_in[i])
                {
                    memset(&rs485_buffer_[chn], 0, sizeof(RS485Msg));
                    return false;
                }
            }
        }
    }

    rs485_buffer_[chn].is_frame_part_in[cur_frame_id] = true;
    int data_index = 0;
    for (uint8_t i = 2; i < len; i++)  // 有效数据从第三字节开始
    {
        data_index = 6 * (cur_frame_id - 1) + i - 2;
        rs485_buffer_[chn].tio_rs485_buf[data_index] = data[i];
        rs485_buffer_[chn].data_len++;
    }

    for (int i = 1; i <= whole_frame_size; i++)
    {
        if (!rs485_buffer_[chn].is_frame_part_in[i])
        {
            // 并非所有数据ready
            return false;
        }
    }
    //
    for (int i = 1; i <= whole_frame_size; i++) { rs485_buffer_[chn].is_frame_part_in[i] = false; }
    return true;
}

int TioV3UsrIoItf::procRs485ResponseData(int chn_id, uint8_t* buf, int len)
{
    if (buf == NULL)
        return -1;
    if (chn_id == 3)
        return -1;
    if (stat_rs485cfg_.rs485_mode_cfg[chn_id] == rtdev::RS485_TRANS && stat_rs485cfg_.rs485_sensor_chn == 0)
    {
        //判断为透传消息,入缓冲
        for (int i = 0; i < len; i++)
        {
            rs485_trans_buffer_[chn_id].push_back(buf[i]);  //boost::circle_buffer超大小后会覆盖旧数据，出缓冲的时候需要分别读取和删除
        }
    }
    else if (stat_rs485cfg_.rs485_mode_cfg[chn_id] == rtdev::RS485_TRANS && stat_rs485cfg_.rs485_sensor_chn == (uint32_t)(chn_id + 1))
    {
        //TODO: 读力传感器
        for (int i = 0; i < len; i++)
        {
            rs485_trans_buffer_[chn_id].push_back(buf[i]);  //boost::circle_buffer超大小后会覆盖旧数据，出缓冲的时候需要分别读取和删除
        }
    }
    else if (stat_rs485cfg_.rs485_mode_cfg[chn_id] == rtdev::RS485_RTU)
    {
        //TODO: 读信号量
    }
    return 0;
}

int TioV3Board::init(const char* cfgfile)
{
    Util::ConfigParser cfg(cfgfile);
    int sflag = 0;
    cfg.get("tio.sim", sflag);
    sim_flag_ = sflag;

    return 0;
}
int TioV3Board::boot()
{
    auto bus = rtbus::BusMgr::get()->bus("robbus0");
    if (!bus)
        return -1;
    bus->config_rxpdo(TIO_STATUS_R, 8);  //PDO
    bus->config_txpdo(TIO_STATUS_W, 8);
    bus->config_rxpdo(TIO_RS485_2_R, 8);
    OptionalPdoMap_[TIO_RS485_2_R] = {0, 1000, true};
    bus->config_txpdo(TIO_RS485_2_W, 8);
    bus->config_rxpdo(TIO_RS485_1_R, 8);
    OptionalPdoMap_[TIO_RS485_1_R] = {0, 1000, true};
    bus->config_txpdo(TIO_RS485_1_W, 8);
    bus->config_rxpdo(TIO_RS485_3_R, 8);
    OptionalPdoMap_[TIO_RS485_3_R] = {0, 1000, true};

    bus->register_sdo_handle(0xff, TIO_PARAM_CFG_W, TIO_PARAM_CFG_W, TIO_PARAM_CFG_R, 2);  //SDO
    bus_ = bus;

    // 读取软件状态以确认启动成功
    int err = sysio__.query_Tio_version();
    if (err)
    {
        return err;
    }
    return 0;
}

int TioV3Board::set_send_data(int busid)
{
    check_alive();
    check_alive_optional();
    if (!bus_)
        return -1;

    uint8_t data[8] = {0};
    data[0] = usrio__.cmd_.set_do;
    data[1] = sysio__.cmd_RGB_;
    bus_->set_txpdo_data(TIO_STATUS_W, data);

    if (usrio__.stat_.ai_config == 0xff)
    {
        //todo 解析485
    }
    if (usrio__.stat_.do_config == 0xff)
    {
        //todo 解析485
    }
    //todo 根据配置补充485部分

    return 0;
}

int TioV3Board::get_recv_data(int busid)
{
    if (!bus_)
        return -1;

    uint8_t data[8] = {0};
    auto alive_cnt_190 = bus_->get_rxpdo_data(TIO_STATUS_R, data);
    if (keep_alive(alive_cnt_190))
    {
        return -1;
    }
    sysio__.fb_keys_.UIKey_free = data[0] & 0b0001;
    sysio__.fb_keys_.UIKey_point = data[0] & 0b0010;
    sysio__.fb_keys_.UIKey_pause_resume = data[0] & 0b0100;
    usrio__.stat_.di_buffer = data[1];
    usrio__.stat_.do_buffer = data[2];
    usrio__.stat_.ai[1] = (data[3] << 8) + data[4];
    usrio__.stat_.ai[0] = (data[5] << 8) + data[6];
    sysio__.fb_RGB_ = (data[7] >> 4) & 0xf;
    usrio__.stat_.is_overload = data[7] & 0b0001;
    uint8_t rs485data[40] = {0};
    int tmp_chn_id = 0;
    auto alive_cnt_730 = bus_->get_rxpdo_data(TIO_RS485_1_R, rs485data);  // TODO alive_cnt 在多pdo的时候表现应该是什么？
    // 根据rs485_mode_cfg, 决定data解析模式,
    if (keep_alive_optional(TIO_RS485_1_R, alive_cnt_730))
    {
        //TODO: RS485_1_R 中断，日志
    }
    tmp_chn_id = 0;
    if (usrio__.collect_frame(tmp_chn_id, rs485data, 8))
    {
        usrio__.procRs485ResponseData(tmp_chn_id, usrio__.rs485_buffer_[tmp_chn_id].tio_rs485_buf, usrio__.rs485_buffer_[tmp_chn_id].data_len);
    }
    memset(rs485data, 0, sizeof(rs485data));
    auto alive_cnt_720 = bus_->get_rxpdo_data(TIO_RS485_2_R, rs485data);
    if (keep_alive_optional(TIO_RS485_2_R, alive_cnt_720))
    {
        //TODO: RS485_2_R 中断，日志
    }
    tmp_chn_id = 1;
    if (usrio__.collect_frame(tmp_chn_id, rs485data, 8))
    {
        usrio__.procRs485ResponseData(tmp_chn_id, usrio__.rs485_buffer_[tmp_chn_id].tio_rs485_buf, usrio__.rs485_buffer_[tmp_chn_id].data_len);
    }
    memset(rs485data, 0, sizeof(rs485data));
    // TODO 根据rs485_mode_cfg, 决定data解析模式
    auto alive_cnt_740 = bus_->get_rxpdo_data(TIO_RS485_3_R, rs485data);  // TODO 多包时，尾包不一定是固定8bytes会有异常吗？
    // TODO 按照sensor type 6解析
    if (keep_alive_optional(TIO_RS485_3_R, alive_cnt_740))
    {
        //TODO: RS485_2_R 中断，日志
    }
    return 0;
}

int TioV3Board::get_sdo_blocking(uint32_t objid, void* data, int s, int retry)
{
    if (!bus_)
        return -1;
    return bus_->get_sdo_blocking(0xff, objid, data, s, retry);
}

int TioV3Board::set_sdo_blocking(uint32_t objid, uint32_t data, size_t s, uint32_t* readback, int retry)
{
    if (!bus_)
        return -1;
    return bus_->set_sdo_blocking(0xff, objid, &data, s, readback, retry);
}

TioBoardItf* rtdev::TioV3Factory::create() { return new TioV3Board(); }